﻿
#include "transform_def.h"
#include <graphics/imath.h>
#include <klog.h>

using namespace DirectX;

namespace ifire {
Trans3 Trans3::EMPTY = Trans3();

Trans2::Trans2(FXMMATRIX mat) {
  XMVECTOR scale;
  XMMatrixDecompose(&scale, &rotation, &position, mat);
}
Trans2::Trans2(const XMFLOAT4X4& mat) {
  XMVECTOR scale;
  XMMatrixDecompose(&scale, &rotation, &position, XMLoadFloat4x4(&mat));
}

IVec3 Trans2::GetPosition() const {
  IVec3 vec;
  XMStoreFloat3(&vec, position);
  return vec;
}

IVec4 Trans2::GetRotation() const {
  IVec4 vec;
  XMStoreFloat4(&vec, rotation);
  return vec;
}

IVec3 Trans2::GetRotationEuler() const {
  return imath::QuaternionToEulerDegree(rotation);
}

Trans2 Trans2::Minus(const Trans2& b) const {
  Trans2 diff;
  diff.position = XMVectorSubtract(position, b.position);

  // 计算四元数的差值
  XMVECTOR q_a_inv = XMQuaternionInverse(rotation);
  diff.rotation = XMQuaternionMultiply(b.rotation, q_a_inv);

  return diff;
}

Trans2 Trans2::Add(const Trans2& b) const {
  Trans2 diff;

  diff.position = XMVectorAdd(position, b.position);
  // 四元数乘法等于相加
  diff.rotation = XMQuaternionMultiply(rotation, b.rotation);

  return diff;
}

void Trans2::Reset() {
  position = XMVectorSet(0.0f, 0.0f, 0.0f, 0.0f);
  rotation = XMVectorSet(0.0f, 0.0f, 0.0f, 1.0f);
}

XMMATRIX Trans2::GetMatrix() const {
  return XMMatrixAffineTransformation(SCALE1, VEC0, rotation, position);
}

void Trans2::Print(const char* format, ...) const {
  va_list ap;
  va_start(ap, format);
  ikit::printv(format, ap);
  va_end(ap);

  auto p = GetPosition();
  auto r = GetRotation();
  KLOG("POS(%.3f,%.3f,%.3f) ROT(%.0f,%.0f,%.0f)", p.x, p.y, p.z, r.x, r.y, r.z);
}

Trans3::Trans3()
    : position(DirectX::XMVectorZero())
    , rotation(DirectX::XMVectorSet(0.0f, 0.0f, 0.0f, 1.0f))
    , scale(DirectX::XMVectorSet(1.0f, 1.0f, 1.0f, 0.0f)) {}

Trans3::Trans3(IVec3 _position, IVec4 _rotation, IVec3 _scale)
    : position(DirectX::XMLoadFloat3(&_position))
    , rotation(DirectX::XMLoadFloat4(&_rotation))
    , scale(DirectX::XMLoadFloat3(&_scale)) {}

Trans3::Trans3(IVec3 _position, IVec3 _rotation, IVec3 _scale)
    : position(DirectX::XMLoadFloat3(&_position))
    , rotation(imath::EulerToQuaternion(DirectX::XMLoadFloat3(&_rotation)))
    , scale(DirectX::XMLoadFloat3(&_scale)) {}

Trans3::Trans3(const ikit::XmlNode& node) {
  XMFLOAT3 pos(node("Position"));
  XMFLOAT3 rot(node("Rotation"));
  XMFLOAT3 scl(node("Scale", XMFLOAT3(1.0f, 1.0f, 1.0f)));
  position = XMLoadFloat3(&pos);
  rotation = imath::EulerToQuaternion(XMLoadFloat3(&rot));
  scale = XMLoadFloat3(&scl);
}

Trans3::Trans3(IVec _position, IVec _rotation, IVec _scale)
    : position(_position), rotation(_rotation), scale(_scale) {}

Trans3::Trans3(const Trans3& other)
    : position(other.position), rotation(other.rotation), scale(other.scale) {}

Trans3::Trans3(FMat mat) {
  XMMatrixDecompose(&scale, &rotation, &position, mat);
}

Trans3::Trans3(FMat4 mat) {
  XMMatrixDecompose(&scale, &rotation, &position, XMLoadFloat4x4(&mat));
}

IVec3 Trans3::GetPosition() const {
  IVec3 vec;
  XMStoreFloat3(&vec, position);
  return vec;
}

IVec4 Trans3::GetRotation() const {
  IVec4 vec;
  XMStoreFloat4(&vec, rotation);
  return vec;
}

IVec3 Trans3::GetRotationEuler() const {
  return imath::QuaternionToEulerDegree(rotation);
}

IVec3 Trans3::GetScale() const {
  IVec3 vec;
  XMStoreFloat3(&vec, scale);
  return vec;
}

Trans3 Trans3::Minus(const Trans3& b) const {
  Trans3 diff;
  diff.position = XMVectorSubtract(position, b.position);
  diff.scale = XMVectorSubtract(position, b.position);

  // 计算四元数的差值
  XMVECTOR q_a_inv = XMQuaternionInverse(rotation);
  diff.rotation = XMQuaternionMultiply(b.rotation, q_a_inv);

  return diff;
}

Trans3 Trans3::Add(const Trans3& b) const {
  Trans3 diff;

  diff.position = XMVectorAdd(position, b.position);
  diff.scale = XMVectorAdd(scale, b.scale);
  // 四元数乘法等于相加
  diff.rotation = XMQuaternionMultiply(rotation, b.rotation);

  return diff;
}

void Trans3::Reset() {
  position = XMVectorSet(0.0f, 0.0f, 0.0f, 0.0f);
  rotation = XMVectorSet(0.0f, 0.0f, 0.0f, 1.0f);
  scale = XMVectorSet(1.0f, 1.0f, 1.0f, 0.0f);
}

XMMATRIX Trans3::GetMatrix() const {
  return XMMatrixAffineTransformation(
      scale, XMVectorZero(), rotation, position);
}

void Trans3::Print(const char* format, ...) const {
  va_list ap;
  va_start(ap, format);
  ikit::printv(format, ap);
  va_end(ap);

  auto p = GetPosition();
  auto r = GetRotationEuler();
  auto s = GetScale();
  ikit::print("\tP[% 5.3f,% 5.3f,% 5.3f]\tR[% 4.0f,% 4.0f,% "
              "4.0f]\tS[%.2g,%.2g,%.2g]\n", //
      p.x, p.y, p.z, r.x, r.y, r.z, s.x, s.y, s.z);
}

void Trans3::Print(ikit::file_printer& f, const char* format, ...) const {
  va_list ap;
  va_start(ap, format);
  f.printv(format, ap);
  va_end(ap);

  auto p = GetPosition();
  auto r = GetRotationEuler();
  auto s = GetScale();
  f.print("\tP[% 5.3f,% 5.3f,% 5.3f]\tR[% 4.0f,% 4.0f,% "
          "4.0f]\tS[%.1g,%.1g,%.1g]\n", //
      p.x, p.y, p.z, r.x, r.y, r.z, s.x, s.y, s.z);
}

IVec3 XMVEC::to_euler() const { return imath::QuaternionToEuler(vec); }

IVec XMVEC::to_quat() const { return imath::EulerToQuaternion(vec); }

} // namespace ifire